#include <cmath>
#include "Vector3D.h"

namespace Maths
{

const Vector3D Vector3D::ZERO(0,0,0);

Vector3D::Vector3D(void)
{
	m_X = 0;
	m_Y = 0;
	m_Z = 0;
}

Vector3D::Vector3D(float x, float y, float z)
{
	m_X = x;
	m_Y = y;
	m_Z = z;
}

Vector3D::Vector3D(const float* vector)
{
	m_X = vector[0];
	m_Y = vector[1];
	m_Z = vector[2];
}

Vector3D::~Vector3D(void)
{
}

float& Vector3D::X()
{
	return m_X;
}

const float& Vector3D::X() const
{
	return m_X;
}


float& Vector3D::Y()
{
	return m_Y;
}

const float& Vector3D::Y() const
{
	return m_Y;
}

float& Vector3D::Z()
{
	return m_Z;
}

const float& Vector3D::Z() const
{
	return m_Z;
}


Vector3D& Vector3D::operator-= (const Vector3D& vec)
{
	m_X -= vec.m_X;
	m_Y -= vec.m_Y;
	m_Z -= vec.m_Z;
	return *this;
};
Vector3D& Vector3D::operator= (const Vector3D& vec)
{
	m_X = vec.m_X;
	m_Y = vec.m_Y;
	m_Z = vec.m_Z;
	return *this;
};
Vector3D& Vector3D::operator+= (const Vector3D& vec)
{
	m_X += vec.m_X;
	m_Y += vec.m_Y;
	m_Z += vec.m_Z;
	return *this;
};
	
float Vector3D::Magnitude() const
{
	return sqrtf(MagnitudeSq());
}

float Vector3D::MagnitudeSq() const
{
	return (m_X * m_X) + (m_Y * m_Y) + (m_Z * m_Z);
}

Vector3D Vector3D::Inverse() const
{
	return Vector3D(-m_X, -m_Y, -m_Z);
}
void Vector3D::Invert()
{
	m_X = -m_X;
	m_Y = -m_Y;
	m_Z = -m_Z;
}
void Vector3D::Set(float x, float y, float z)
{
	m_X = x;
	m_Y = y;
	m_Z = z;
};
Vector3D Vector3D::operator- (const Vector3D& vec) const
{
	Vector3D vector(*this);
	vector -= vec;
	return vector;
}
Vector3D Vector3D::operator+ (const Vector3D& vec) const
{
	Vector3D vector(vec);
	vector += *this;
	return vector;
}
Vector3D& Vector3D::operator/= (const Vector3D& vec)
{
	m_X /= vec.m_X;
	m_Y /= vec.m_Y;
	m_Z /= vec.m_Z;
	return *this;
}
Vector3D& Vector3D::operator*= (const Vector3D& vec)
{
	m_X *= vec.m_X;
	m_Y *= vec.m_Y;
	m_Z *= vec.m_Z;
	return *this;
};

Vector3D Vector3D::operator/ (const Vector3D& vec) const
{
	Vector3D vector(*this);
	vector /= vec;
	return vector;
};
Vector3D Vector3D::operator* (const Vector3D& vec) const
{
	Vector3D vector(vec);
	vector *= *this;
	return vector;
};
Vector3D& Vector3D::operator-= (const float& o)
{
	m_X -= o;
	m_Y -= o;
	m_Z -= o;
	return *this;
};
Vector3D& Vector3D::operator+= (const float& o)
{
	m_X += o;
	m_Y += o;
	m_Z += o;
	return *this;
};

Vector3D Vector3D::operator- (const float& o) const
{
	Vector3D vector(*this);
	vector -= o;
	return vector;
};
Vector3D Vector3D::operator+ (const float& o) const
{
	Vector3D vector(*this);
	vector += o;
	return vector;
};
Vector3D& Vector3D::operator/= (const float& o)
{
	m_X /= o;
	m_Y /= o;
	m_Z /= o;
	return *this;
};
Vector3D& Vector3D::operator*= (const float& o)
{
	m_X *= o;
	m_Y *= o;
	m_Z *= o;
	return *this;
};

Vector3D Vector3D::operator/ (const float& o) const
{
	Vector3D vector(*this);
	vector /= o;
	return vector;
};
Vector3D Vector3D::operator* (const float& o) const
{
	Vector3D vector(*this);
	vector *= o;
	return vector;
};
void Vector3D::Zero()
{
	m_X = 0;
	m_Y = 0;
	m_Z = 0;
}

void Vector3D::Rotate(Vector3D& angles)
{
	float &x = m_X, x1;
	float &y = m_Y, y1;
	float &z = m_Z, z1;

	float cosa = cos(angles.m_Y);
	float sina = sin(angles.m_Y);

	x1 = -(sina*z)+(cosa*x);
	y1 = y;
	z1 = -(sina*x)-(cosa*z);

	m_X = x1;
	m_Y = y1;
	m_Z = z1;
}

void Vector3D::Translate(Vector3D& translation)
{
	*this += translation;
}
// Functions
void Vector3D::Normalize( )
{
	float length = Length();
	m_X /= length;
	m_Y /= length;
	m_Z /= length;
}

float Dot(const float* vector1, const float* vector2)
{
	return (vector1[0]*vector2[0])+(vector1[1]*vector2[1])+(vector1[2]*vector2[2]);
};

float Vector3D::Dot(const Vector3D& vector2) const
{
	return (m_X*vector2.m_X)+(m_Y*vector2.m_Y)+(m_Z*vector2.m_Z);
};


//inline Vec4 Vector3D::Cross(const Vec4 &c)
//{
//	Vec4 res;
//	res.x = m_Y*c.z - c.y*m_Z;
//	res.y = m_Z*c.x - c.z*m_X;
//	res.z = m_X*c.y - c.x*m_Z;
//	res.w = 0;
//	return res;
//}
//
//inline Vec4 Vector3D::asm_Cross(const Vec4 &b, const Vec4 &c)
//{
//	Vec4 res;
//	__asm
//	{
//		movups xmm0, b		//  vector 1
//		movups xmm1, c		//  vector 2
//		movaps xmm2, xmm0
//		movaps xmm3, xmm1
//
//		shufps xmm2, xmm0, 0xc9
//		shufps xmm3, xmm1, 0xd2
//		shufps xmm1, xmm1, 0xc9
//		shufps xmm0, xmm0, 0xd2
//
//		mulps xmm2, xmm3
//		mulps xmm1, xmm0
//		subps xmm2, xmm1
//
//		movups res, xmm2
//	}
//	res.w = 0;
//	return res;
//}

Vector3D Vector3D::Cross(const Vector3D &c)
{
	Vector3D res;
	res.m_X = m_Y*c.m_Z - c.m_Y*m_Z;
	res.m_Y = m_Z*c.m_X - c.m_Z*m_X;
	res.m_Z = m_X*c.m_Y - c.m_X*m_Y;
	return res;
}

Vector3D Vector3D::asm_Cross(const Vector3D &c)
{
	Vector3D res;
	Vector3D v1, v2;
	v1.Zero();
	v2.Zero();
	v1.Set(m_X, m_Y, m_Z);
	v2.Set(c.m_X, c.m_Y, c.m_Z);
	__asm
	{
		movups xmm0, v1		//  vector 1
		movups xmm1, v2		//  vector 2
		movaps xmm2, xmm0
		movaps xmm3, xmm1

		shufps xmm2, xmm0, 0xc9
		shufps xmm3, xmm1, 0xd2
		shufps xmm1, xmm1, 0xc9
		shufps xmm0, xmm0, 0xd2

		mulps xmm2, xmm3
		mulps xmm1, xmm0
		subps xmm2, xmm1

		movups res, xmm2
	}
	return res;
}
float Vector3D::Distance( Vector3D &o )
{
	Vector3D temp = o - *this;

	return temp.Length();
};

float Vector3D::DistanceSqr( Vector3D &o )
{
	Vector3D temp = o - *this;

	return temp.LengthSqr();
};
float Vector3D::Length()
{
	return sqrt(this->Dot(*this));
}
float Vector3D::LengthSqr()
{
	return this->Dot(*this);
}

bool Vector3D::operator!= (const Vector3D& o) const
{
	if (m_X != o.m_X && m_Y != o.m_Y && m_Z != o.m_Z)
		return true;
	return false;
}

bool Vector3D::operator== (const Vector3D& o) const
{
	if (m_X == o.m_X && m_Y == o.m_Y && m_Z == o.m_Z)
		return true;
	return false;
}

Maths::Vector3D Maths::Vector3D::Normal() const
{
	Maths::Vector3D normal = *this;
	normal.Normalize();
	return normal;
}
};
